﻿using DroneLib.Communication;
using DroneLib.Vehicle;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Net;
using System.Runtime.InteropServices.ComTypes;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using static MAVLink;

namespace DroneLib.Vehicles
{
    partial class Vehicle
    {
        public static async Task<(bool connected,Vehicle vehicle,string error)> Connect(string connString,bool fastSearch=false, byte gcsId = 255, int timeout=30000,CancellationTokenSource cts = null)
        {
            MavStream stream = null;
            string conn=connString.ToLower();
            bool isUdpClient = false;
            if (conn.StartsWith("udp://"))
            {
                var items = conn.Substring(6).Split(':');
                IPAddress ip;
                int port;

                if (items.Length != 2)
                    return (false, null, "参数不正确");

                if (IPAddress.TryParse(items[0], out ip) == false)
                    return (false, null, "参数不正确");

                if (Int32.TryParse(items[1], out port) == false)
                    return (false, null, "参数不正确");

                stream = new UdpStream(ip, port);
            }
            else if (conn.StartsWith("udpcl://"))
            {
                var items = conn.Substring(8).Split(':');
                IPAddress ip;
                int port;

                if (items.Length != 2)
                    return (false, null, "参数不正确");

                if (IPAddress.TryParse(items[0], out ip) == false)
                    return (false, null, "参数不正确");

                if (Int32.TryParse(items[1], out port) == false)
                    return (false, null, "参数不正确");

                stream = new UdpStream(ip, port, true);
                isUdpClient = true;
            }
            else if (conn.StartsWith("tcp://"))
            {
                var items = conn.Substring(6).Split(':');

                IPAddress ip;
                int port;

                if (items.Length != 2)
                    return (false, null, "参数不正确");

                if (IPAddress.TryParse(items[0], out ip) == false)
                    return (false, null, "参数不正确");

                if (Int32.TryParse(items[1], out port) == false)
                    return (false, null, "参数不正确");

                stream = new TcpStream(ip, port);
            }
            else if (conn.StartsWith("serial://"))
            {
                var items = conn.Substring(9).Split(':');

                int baudRate;
                if (Int32.TryParse(items[1], out baudRate) == false)
                    return (false, null, "参数不正确");

                stream = new SerialStream(items[0], baudRate);
            }

            if (stream == null)
                return (false, null, "无法创建 MavLink 流");

            stream.GcsID = gcsId;
            try
            {
                stream.Open();
            }
            catch(Exception ex)
            {
                try
                {
                    stream.Close();
                }
                catch { }
                return (false, null, ex.Message);
            }
            if (stream is TcpStream)
            {
                DateTime start = DateTime.Now;
                try
                {
                    while (DateTime.Now.Subtract(start).TotalSeconds < 10)
                    {
                        if (stream.IsOpen || ((TcpStream)stream).ConnectionFailed)
                            break;

                        await Task.Delay(100);
                    }
                }
                catch(Exception ex) 
                {
                }
            }

            if (stream.IsOpen)
            {
                if(isUdpClient)
                {
                    MAVLink.MavlinkParse parse = new MavlinkParse();

                    mavlink_heartbeat_t hb = new mavlink_heartbeat_t()
                    {
                        autopilot = 8,
                        mavlink_version = 3,
                        type = 6
                    };

                    mavlink_request_data_stream_t req = new mavlink_request_data_stream_t()
                    {
                        req_message_rate = 2,
                        req_stream_id = 2,
                        start_stop = 1,
                        target_component = 0,
                        target_system = 0
                    };

                    byte[] hbBytes = parse.GenerateMAVLinkPacket10(MAVLINK_MSG_ID.HEARTBEAT, hb);
                    stream.Write(hbBytes, 0, hbBytes.Length);
                    await Task.Delay(10);
                    byte[] reqBytes = parse.GenerateMAVLinkPacket10(MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
                    stream.Write(reqBytes,0,reqBytes.Length);
                    await Task.Delay(10);
                    stream.Write(reqBytes, 0, reqBytes.Length);
                    await Task.Delay(1000);

                    hbBytes = parse.GenerateMAVLinkPacket20(MAVLINK_MSG_ID.HEARTBEAT, hb);
                    stream.Write(hbBytes, 0, hbBytes.Length);
                    await Task.Delay(10);
                    reqBytes = parse.GenerateMAVLinkPacket20(MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
                    stream.Write(reqBytes, 0, reqBytes.Length);
                    await Task.Delay(10);
                    stream.Write(reqBytes, 0, reqBytes.Length);
                    await Task.Delay(10);
                }

                if (fastSearch)
                {
                    var result =await Search(stream, timeout, 1, cts);
                    if (result == null)
                        stream.Close();
                    return (result != null, result, result == null ? "连接超时，没有发现载具，或已连接了具有相同 SYSID 的载具" : string.Empty);
                }
                else
                {
                    var result =await Search(stream, timeout,5, cts);
                    if (result == null)
                        stream.Close();
                    return (result != null, result, result == null ? "连接超时，没有发现载具，或已连接了具有相同 SYSID 的载具" : string.Empty);
                }
            }
            else
            {
                return (false, null, stream.Error);
            }
        }

        public void Reconnect()
        {
            if (_MavStream != null)
            {
                _MavStream.Reconnect();
            }
        }

        static bool VehicleConnected(byte sysId,MavStream stream)
        {
            return _KnownVehicles.Values.Where(v => v.SysId == sysId && v._MavStream.Equals(stream)).Count() > 0;
        }

        static async Task<Vehicle> Search(MavStream stream, int timeout, int waitHeartbeatCount = 5, CancellationTokenSource cancel=null)
        {
            bool result = false;
            var retryTimeout = new RetryTimeout(1, timeout);
            Vehicle vehicle = null;
            Dictionary<byte, int> heartbeatCount = new Dictionary<byte, int>();
            Dictionary<byte, MAVLink.mavlink_heartbeat_t> messages = new Dictionary<byte, mavlink_heartbeat_t>();
            Func<MAVLinkMessage, bool> waitHeartbeat = (msg) =>
            {
                MAVLink.mavlink_heartbeat_t heartbeat = (MAVLink.mavlink_heartbeat_t)msg.data;

                MAVLink.MAV_TYPE mavType = MAV_TYPE.GCS;
                try
                {
                    mavType = (MAVLink.MAV_TYPE)((MAVLink.mavlink_heartbeat_t)msg.data).type;
                }
                catch(Exception ex)
                { 
                }
                if (mavType == MAV_TYPE.GCS || mavType == MAVLink.MAV_TYPE.ADSB)
                    return false;

                if (VehicleConnected(msg.sysid, stream)==false)
                {
                    if (heartbeatCount.ContainsKey(msg.sysid) == false)
                        heartbeatCount[msg.sysid] = 0;
                    heartbeatCount[msg.sysid]++;
                    messages[msg.sysid] = heartbeat;
                    retryTimeout.ResetTimeout();
                }

                var anyValid = heartbeatCount.Where(hbCount => hbCount.Value >= waitHeartbeatCount).ToArray();

                if (anyValid.Length == 0)
                    return false;

                stream.MavlinkVersion = msg.ismavlink2 ? 2 : 1;

                byte sysId = anyValid[0].Key;
                var hb = messages[sysId];
                var v = Vehicle.Create(sysId, (MAV_TYPE)hb.type, stream, hb.custom_mode);
                if (v is null)
                {
                    return false;
                }

                v.MavlinkVersion = msg.ismavlink2 ? 2 : 1;
                _KnownVehicles[v.Guid] = v;
                MAV_AUTOPILOT apType = (MAV_AUTOPILOT)hb.autopilot;
                switch (apType)
                {
                    case MAV_AUTOPILOT.ARDUPILOTMEGA:
                        {
                            switch (v.Type)
                            {
                                case MAV_TYPE.FIXED_WING:
                                    v.Firmware = Firmwares.ArduPlane;
                                    break;

                                case MAV_TYPE n when (n >= MAV_TYPE.VTOL_TAILSITTER_DUOROTOR && n <= MAV_TYPE.VTOL_RESERVED5):
                                    v.Firmware = Firmwares.ArduPlane;
                                    break;

                                case MAV_TYPE.FLAPPING_WING:
                                    v.Firmware = Firmwares.ArduPlane;
                                    break;

                                case MAV_TYPE.QUADROTOR:
                                    v.Firmware = Firmwares.ArduCopter2;
                                    break;

                                case MAV_TYPE.TRICOPTER:
                                    v.Firmware = Firmwares.ArduCopter2;
                                    break;

                                case MAV_TYPE.HEXAROTOR:
                                    v.Firmware = Firmwares.ArduCopter2;
                                    break;

                                case MAV_TYPE.OCTOROTOR:
                                    v.Firmware = Firmwares.ArduCopter2;
                                    break;

                                case MAV_TYPE.HELICOPTER:
                                    v.Firmware = Firmwares.ArduCopter2;
                                    break;

                                case MAV_TYPE.DODECAROTOR:
                                    v.Firmware = Firmwares.ArduCopter2;
                                    break;

                                case MAV_TYPE.GROUND_ROVER:
                                    v.Firmware = Firmwares.ArduRover;
                                    break;

                                case MAV_TYPE.SURFACE_BOAT:
                                    v.Firmware = Firmwares.ArduRover;
                                    break;

                                case MAV_TYPE.SUBMARINE:
                                    v.Firmware = Firmwares.ArduSub;
                                    break;

                                case MAV_TYPE.ANTENNA_TRACKER:
                                    v.Firmware = Firmwares.ArduTracker;
                                    break;
                                case MAV_TYPE.COAXIAL:
                                    v.Firmware = Firmwares.ArduCopter2;
                                    break;
                                default:
                                    v.Firmware = Firmwares.Other;
                                    break;
                            }
                            break;
                        }
                    case MAV_AUTOPILOT.UDB:
                        {
                            switch (v.Type)
                            {
                                case MAV_TYPE.FIXED_WING:
                                    v.Firmware = Firmwares.ArduPlane;
                                    break;
                            }
                            break;
                        }
                    case MAV_AUTOPILOT.GENERIC:
                        {
                            switch (v.Type)
                            {
                                case MAV_TYPE.FIXED_WING:
                                    v.Firmware = Firmwares.Ateryx;
                                    break;
                            }
                            break;
                        }
                    case MAV_AUTOPILOT.PX4:
                        {
                            v.Firmware = Firmwares.PX4;
                            break;
                        }
                    default:
                        {
                            switch (v.Type)
                            {
                                case MAV_TYPE.GIMBAL: // storm32 - name 83
                                    v.Firmware = Firmwares.Gimbal;
                                    break;
                            }

                            break;
                        }
                }

                vehicle = v;
                retryTimeout.Complete = true;
                return true;
            };

            int id = stream.SubscribeMessage(MAVLINK_MSG_ID.HEARTBEAT, waitHeartbeat, 0, 0);
            
            retryTimeout.WorkToDoAsync = async() => 
            {
                if (cancel != null && cancel.IsCancellationRequested)
                {
                    retryTimeout.RetriesCurrent = 999;
                    Console.WriteLine($"Search Canceled");
                    return false;
                }
                return true;
            };
            result = await retryTimeout.DoWorkAsync();

            stream.UnsubcribeMessage(id);
            return vehicle;
        }
    }
}
